

#include <geometry_msgs/msg/pose2_d.hpp>
#include <ipa_building_navigation/ipa_room_planning.h>


int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto nh = rclcpp::Node::make_shared("ipa_room_planning_node");
	SimpleActionClientD<ClientSegmentation> room_segmentation_client(nh,std::string("room_segmentation_server"));
	SimpleActionClientD<ClientSequence> 	room_sequence_client(nh,std::string("room_sequence_planning_server"));
	SimpleActionClientD<ClientExploration> 	room_exploration_client(nh,std::string("room_exploration_server"));
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr all_path_pub;
    all_path_pub = nh->create_publisher<nav_msgs::msg::Path>("all_coverage_path", 1);

    std::string  map_name = "map2.pgm";  //  map2.pgm lab_ipa.png  freiburg_building101.png
    std::string image_filename = "/home/ora/ros2_ws/src/Sweeping_robot/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/" + map_name;
    std::cout<< "current pic is : "<< image_filename<< std::endl;
    cv::Mat map = cv::imread(image_filename.c_str(), 0);
    std::vector<double> map_origin (3,0);
	map_origin[0]= 0;
	map_origin[1]= 0;
    std::vector<double> start_pos = {0, 0, 0};
    geometry_msgs::msg::Pose2D starting_position;
	starting_position.x = start_pos[0];
	starting_position.y = start_pos[1];
	starting_position.theta = start_pos[2];
    double map_resolution = 0.05;
    double robot_radius = 0.15;
    double coverage_radius=0.15;
    int planning_mode=1;
    std::vector<geometry_msgs::msg::Point32> fov_points(4);
    fov_points[0].x = -0.15;		// this is the working area of a vacuum cleaner with 60 cm width
	fov_points[0].y = 0.15;
	fov_points[1].x = -0.15;
	fov_points[1].y = -0.15;
	fov_points[2].x = 0.15;
	fov_points[2].y = -0.15;
	fov_points[3].x = 0.15;
	fov_points[3].y = 0.15;
    geometry_msgs::msg::Point32 fov_origin;
	fov_origin.x = 0.;
	fov_origin.y = 0.;

    
    //make non-white pixels black
    for (int y = 0; y < map.rows; y++)
    {
        for (int x = 0; x < map.cols; x++)
        {
            if (map.at<unsigned char>(y, x) < 250)
            {
                map.at<unsigned char>(y, x) = 0;
            }
            else
            {
                map.at<unsigned char>(y, x) = 255;
            }
        }
    }
    sensor_msgs::msg::Image map_msg;
    cv_bridge::CvImage cv_image;
    cv_image.encoding = "mono8";
    cv_image.image = map;
    cv_image.toImageMsg(map_msg);
    while(rclcpp::ok() && !room_segmentation_client.wait_server(5)) 
    {
        std::cout<<"room_segmentation_client Action server not available after waiting, wait 5s..."<< std::endl;
    }
    printf("Action server started, sending goal.\n");
    ipa_building_msgs::action::MapSegmentation::Goal goal_seg;
    goal_seg.data_source=goal_seg.IMAGE_DATA;
    goal_seg.input_map = map_msg;
    goal_seg.map_origin.position.x = map_origin[0];
    goal_seg.map_origin.position.y = map_origin[1];
    goal_seg.map_origin.position.z = map_origin[2];
    goal_seg.map_resolution = map_resolution;
    goal_seg.return_format_in_meter = false;
    goal_seg.return_format_in_pixel = true;
    std::shared_ptr<ClientSegmentation::Result> result_seg= room_segmentation_client.sendGoal(goal_seg);
    if(result_seg== nullptr)
    {
        printf("Timeout on room segmentation.\n");
        return -1;
    }
    printf("segmentation successfully!\n");
    // send a goal_seg to the action
    ipa_building_msgs::action::FindRoomSequenceWithCheckpoints_Goal goal_seq;
    goal_seq.input_map = map_msg;
    goal_seq.map_resolution = goal_seg.map_resolution;
    goal_seq.map_origin.position.x = goal_seg.map_origin.position.x;
    goal_seq.map_origin.position.y = goal_seg.map_origin.position.y;
    goal_seq.room_information_in_pixel = result_seg->room_information_in_pixel;
    goal_seq.robot_radius = robot_radius;
    cv::Mat map_eroded;
    cv::erode(map, map_eroded, cv::Mat(), cv::Point(-1,-1), goal_seq.robot_radius/goal_seq.map_resolution+2);
    cv::Mat distance_map;	//variable for the distance-transformed map, type: CV_32FC1
    cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5);
    cv::convertScaleAbs(distance_map, distance_map);	// conversion to 8 bit image
    while(rclcpp::ok() && !room_sequence_client.wait_server(5)) 
    {
        std::cout<<"room_sequence_client Action server not available after waiting, wait 5s..."<< std::endl;
    }
    bool robot_start_coordinate_set = false;
    for (int v=0; v<map_eroded.rows && robot_start_coordinate_set==false; ++v)
        for (int u=0; u<map_eroded.cols && robot_start_coordinate_set==false; ++u)
            if (map_eroded.at<uchar>(v,u) != 0 && distance_map.at<uchar>(v,u) > 15)
            {
                goal_seq.robot_start_coordinate.position.x = u*goal_seq.map_resolution + goal_seg.map_origin.position.x;
                goal_seq.robot_start_coordinate.position.y = v*goal_seq.map_resolution + goal_seg.map_origin.position.y;
                robot_start_coordinate_set = true;
            }

    std::shared_ptr<ClientSequence::Result> result_seq= room_sequence_client.sendGoal(goal_seq);
    if(result_seq== nullptr)
    {
        printf("Timeout on room segmentation.");
        return -1;
    }
    printf("sequence planning successfully!");

    cv_bridge::CvImagePtr cv_ptr_obj;
    cv_ptr_obj = cv_bridge::toCvCopy(result_seg->segmented_map, sensor_msgs::image_encodings::TYPE_32SC1);
    cv::Mat segmented_map = cv_ptr_obj->image;
    cv::Mat colour_segmented_map = segmented_map.clone();
    colour_segmented_map.convertTo(colour_segmented_map, CV_8U);
    cv::Mat exploration_map = colour_segmented_map.clone();
    
    cv::cvtColor(colour_segmented_map, colour_segmented_map, CV_GRAY2BGR);
    for(size_t i = 1; i <= result_seg->room_information_in_pixel.size(); ++i)
    {
        //choose random color for each room
        int blue = (rand() % 250) + 1;
        int green = (rand() % 250) + 1;
        int red = (rand() % 250) + 1;
        for(size_t u = 0; u < segmented_map.rows; ++u)
        {
            for(size_t v = 0; v < segmented_map.cols; ++v)
            {
                if(segmented_map.at<int>(u,v) == i)
                {
                    colour_segmented_map.at<cv::Vec3b>(u,v)[0] = blue;
                    colour_segmented_map.at<cv::Vec3b>(u,v)[1] = green;
                    colour_segmented_map.at<cv::Vec3b>(u,v)[2] = red;
                }
            }
        }
    }
    cv::Mat path_map = colour_segmented_map.clone();
    cv::flip(path_map, path_map, 0);
    std::vector< std::vector<geometry_msgs::msg::PoseStamped>> all_room_paths;

    std::vector<int> optimal_room_sequence;
    for(int i=0; i< result_seq->checkpoints.size(); i++)
    {
        for(int j=0; j< result_seq->checkpoints[i].room_indices.size(); j++)
        {
            optimal_room_sequence.push_back( result_seq->checkpoints[i].room_indices[j]);
        }
    }

    for(size_t i = 0; i < result_seg->room_information_in_pixel.size(); ++i)
    {
        cv::Mat map_copy(exploration_map == optimal_room_sequence[i]+1);
        cv::flip(map_copy, map_copy, 0);
        for (int y = 0; y < map_copy.rows; y++)
        {
            for (int x = 0; x < map_copy.cols; x++)
            {
                if (map_copy.at<unsigned char>(y, x) > 0)
                {
                    map_copy.at<unsigned char>(y, x) = 255;
                }
                else
                {
                    map_copy.at<unsigned char>(y, x) = 0;
                }
            }
        }
        sensor_msgs::msg::Image labeling;
        cv_bridge::CvImage cv_image;
        cv_image.encoding = "mono8";
        cv_image.image = map_copy;
        cv_image.toImageMsg(labeling);
        geometry_msgs::msg::Pose map_origin_msg;
        map_origin_msg.position.x = map_origin[0];
        map_origin_msg.position.y = map_origin[1];
        map_origin_msg.position.z = map_origin[2];
        geometry_msgs::msg::Pose2D starting_position;
        starting_position.x = result_seg->room_information_in_pixel[optimal_room_sequence[i]].room_center.x;
        starting_position.y = result_seg->room_information_in_pixel[optimal_room_sequence[i]].room_center.y;
        starting_position.theta = 0;
        ipa_building_msgs::action::RoomExploration_Goal goal;
        goal.input_map = labeling;
        goal.map_resolution = map_resolution;
        goal.map_origin = map_origin_msg;
        goal.robot_radius = robot_radius; // turtlebot, used for sim 0.177, 0.4
        goal.coverage_radius = coverage_radius;
        goal.field_of_view = fov_points;
        goal.field_of_view_origin = fov_origin;
        goal.starting_position = starting_position;
        goal.planning_mode = planning_mode;
        while(rclcpp::ok() && !room_exploration_client.wait_server(5)) 
        {
            std::cout<<"room_exploration_client Action server not available after waiting, wait 5s..."<< std::endl;
        }
        printf("Action server started, sending goal.\n");
        std::shared_ptr<ClientExploration::Result> result_exp= room_exploration_client.sendGoal(goal);
        if(result_exp== nullptr)
        {
            printf("Timeout on room segmentation.");
            return -1;
        }
        std::cout << "Got a path with "<<  (int)(result_exp->coverage_path.size()) << " nodes" << std::endl;
        // display path
        const double inverse_map_resolution = 1./goal.map_resolution;
        for (size_t point=0; point<result_exp->coverage_path.size(); ++point)
        {
            const cv::Point point1((result_exp->coverage_path[point].x-map_origin[0])*inverse_map_resolution, (result_exp->coverage_path[point].y-map_origin[1])*inverse_map_resolution);
            cv::circle(path_map, point1, 2, cv::Scalar(128), cv::FILLED);
            if (point > 0)
            {
                const cv::Point point2((result_exp->coverage_path[point-1].x-map_origin[0])*inverse_map_resolution, (result_exp->coverage_path[point-1].y-map_origin[1])*inverse_map_resolution);
                cv::line(path_map, point1, point2, cv::Scalar(128), 1);
            }
            std::cout << "coverage_path[" << point << "]: x=" << result_exp->coverage_path[point].x << ", y=" << result_exp->coverage_path[point].y << ", theta=" << result_exp->coverage_path[point].theta << std::endl;
        }

        all_room_paths.push_back(result_exp->coverage_path_pose_stamped);
        cv::namedWindow("path",0);
        cv::resizeWindow("path", 300, 300);
        cv::imshow("path", path_map);
        cv::waitKey(0);
    }
    nav_msgs::msg::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = rclcpp::Time();
    path_msg.poses.clear();
    for(int i=0; i< all_room_paths.size();i++)
    {
        path_msg.poses.insert(path_msg.poses.end(),all_room_paths[i].begin(),all_room_paths[i].end());
    }
    all_path_pub->publish(path_msg);
	//exit
	return 0;
}
